from pyproj import Transformer
import math

def sim7600_to_decimal(raw_data):
    cleaned = raw_data.split(":")[1].strip()
    parts = cleaned.split(',')

    # 纬度转换
    lat = float(parts[0])
    lat_deg = int(lat // 100)
    lat_min = lat % 100
    lat_wgs84 = lat_deg + lat_min / 60
    if parts[1] == 'S':
        lat_wgs84 = -lat_wgs84

    # 经度转换
    lon = float(parts[2])
    lon_deg = int(lon // 100)
    lon_min = lon % 100
    lon_wgs84 = lon_deg + lon_min / 60
    if parts[3] == 'W':
        lon_wgs84 = -lon_wgs84

    return lat_wgs84, lon_wgs84


def wgs84_to_gcj02(lat, lon):
    """
    WGS-84转GCJ-02（火星坐标系）
    参考算法：https://en.wikipedia.org/wiki/Restrictions_on_geographic_data_in_China
    """
    a = 6378245.0  # 长半轴
    ee = 0.00669342162296594323  # 扁率

    def _transform(x, y):
        dlat = _lat_transform(x - 105, y - 35)
        dlon = _lon_transform(x - 105, y - 35)
        rad_lat = math.radians(y)
        magic = 1 - ee * math.pow(math.sin(rad_lat), 2)
        sqrt_magic = math.sqrt(magic)
        dlat = (dlat * 180.0) / ((a * (1 - ee)) / (magic * sqrt_magic) * math.pi)
        dlon = (dlon * 180.0) / (a / sqrt_magic * math.cos(rad_lat) * math.pi)
        return dlat, dlon

    def _lat_transform(x, y):
        ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * math.sqrt(abs(x))
        ret += (20.0 * math.sin(6.0 * x * math.pi) + 20.0 * math.sin(2.0 * x * math.pi)) * 2.0 / 3.0
        ret += (20.0 * math.sin(y * math.pi) + 40.0 * math.sin(y / 3.0 * math.pi)) * 2.0 / 3.0
        ret += (160.0 * math.sin(y / 12.0 * math.pi) + 320 * math.sin(y * math.pi / 30.0)) * 2.0 / 3.0
        return ret

    def _lon_transform(x, y):
        ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * math.sqrt(abs(x))
        ret += (20.0 * math.sin(6.0 * x * math.pi) + 20.0 * math.sin(2.0 * x * math.pi)) * 2.0 / 3.0
        ret += (20.0 * math.sin(x * math.pi) + 40.0 * math.sin(x / 3.0 * math.pi)) * 2.0 / 3.0
        ret += (150.0 * math.sin(x / 12.0 * math.pi) + 300.0 * math.sin(x / 30.0 * math.pi)) * 2.0 / 3.0
        return ret

    dlat, dlon = _transform(lon, lat)
    return lat + dlat, lon + dlon

def gcj02_to_bd09(lat, lon):
    """
    GCJ-02转BD-09（百度坐标系）
    """
    x_pi = math.pi * 3000.0 / 180.0
    z = math.sqrt(lon * lon + lat * lat) + 0.00002 * math.sin(lat * x_pi)
    theta = math.atan2(lat, lon) + 0.000003 * math.cos(lon * x_pi)
    bd_lon = z * math.cos(theta) + 0.0065
    bd_lat = z * math.sin(theta) + 0.006
    return bd_lat, bd_lon

# 示例数据
raw_data = "+CGPSINFO:4147.993722,N,10959.718333,E,200125,064448.0,1603.5,0.0,252.3"
lat_wgs84, lon_wgs84 = sim7600_to_decimal(raw_data)
lat_gcj02, lon_gcj02 = wgs84_to_gcj02(lat_wgs84, lon_wgs84)
lat_bd09, lon_bd09 = gcj02_to_bd09(lat_gcj02, lon_gcj02)

print(f"高德地图坐标（GCJ-02）：{lat_gcj02}, {lon_gcj02}")
print(f"百度地图坐标（BD-09）：{lat_bd09}, {lon_bd09}")